#include "pid.h"

//#define A 1
//#define B 1  //A,B是根据对象模型具体定制的//对象模型一般抽象为G(s)= B / s * s + A * s
//#define T 1 //采样周期
//#define放在.c文件中


//float Ka = A /  (B * T);
//float Kb = A / (B * T * T);

//FFC myFFC;

int LimitMax(int input,int max)   
{                              
	if(input > max)
	{
		return max;
	}
	else if(input < -max)
	{
		return -max;
	}
}
PidTypedef PidSet(float Kp,float Ki,float Kd,float max_out,float max_iout) 
{
	PidTypedef pid={0};
	
	pid.Kp = Kp;
	pid.Ki = Ki;
	pid.Kd = Kd;
	pid.max_out = max_out;
	pid.max_iout = max_iout;
	
	return pid;
}
/**
  * @brief          电机速度环PID控制
  * @param[in]      *pid: 速度环PID参数  set:设置的目标值  fdb: 目前值
  * @param[out]     返回值为速度环的输出量
  * @author     		 Yang
  */
float PID_V(PidTypedef *pid,float set,float fdb)
{
	if (pid == NULL)
	{
		return 0.0f;
	}
	pid->err_next=pid->err;
	pid->err=set - fdb;
	pid->derr=pid->err-pid->err_next;

	pid->Pout=pid->Kp*pid->err;
	pid->Iout+=pid->Ki*pid->err;
	pid->Iout=LimitMax(pid->Iout,pid->max_iout);
	pid->Dout=pid->Kd*pid->derr;
	
	pid->out=pid->Pout+pid->Iout+pid->Dout;
	pid->out=LimitMax(pid->out,pid->max_out);
	
	return pid->out;
}


/************************************************************************************************************************/
/************************************************************************************************************************/
/**********************************************自写代码*******************************************************************/
/************************************************************************************************************************/
/************************************************************************************************************************/

void PID_Init(PID *pid,float p,float i,float d,float maxI,float maxOut)//初始化PID的PID值，积分限幅和输出限幅
{
    pid->kp=p;
    pid->ki=i;
    pid->kd=d;
    pid->maxIntegral=maxI;
    pid->maxOutput=maxOut;
	pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f;
	pid->error[0] = pid->error[1] = pid->error[2] =pid->output = 0.0f;
}
 
//进行一次pid计算
//参数为(pid结构体,目标值,反馈值)，计算结果放在pid结构体的output成员中
void PID_Calc(PID *pid,float reference,float feedback)
{
 	//更新数据
    pid->error[1]=pid->error[0];//将旧error存起来
    pid->error[0]=reference-feedback;//计算新error
    //计算微分
    float dout=(pid->error[0]-pid->error[1])*pid->kd;
    //计算比例
    float pout=pid->error[0]*pid->kp;
    //计算积分
    pid->integral+=pid->error[0]*pid->ki;
    //积分限幅
    if(pid->integral > pid->maxIntegral) pid->integral=pid->maxIntegral;
    else if(pid->integral < -pid->maxIntegral) pid->integral=-pid->maxIntegral;
    //计算输出
    pid->output=pout+dout+pid->integral;
    //输出限幅
    if(pid->output > pid->maxOutput) pid->output=pid->maxOutput;
    else if(pid->output < -pid->maxOutput) pid->output=-pid->maxOutput;
}



//串级PID的计算函数
//参数(PID结构体,外环目标值,外环反馈值,内环反馈值)
void PID_CascadeCalc(CascadePID *pid,float outerRef,float outerFdb,float innerFdb)
{
    PID_Calc(&pid->outer,outerRef,outerFdb);//计算外环
    PID_Calc(&pid->inner,pid->outer.output,innerFdb);//计算内环
    pid->output=pid->inner.output;//内环输出就是串级PID的输出
}


////增量式PID计算函数
//void increment_pid(PID *pid,float reference,float feedback)
//{
//        pid->Dbuf[2] = pid->Dbuf[1];
//        pid->Dbuf[1] = pid->Dbuf[0];
//        pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);
//        pid->output += pid->kp * (pid->error[0] - pid->error[1]) + 
//		pid->ki * pid->error[0] + 
//		pid->kd * pid->Dbuf[0];
//        LimitMax(pid->output, pid->maxOutput);
//}

// void FeedforwardController(FFC vFFC)//前馈PID
// {
// 	float result;

// 	result = Ka * (vFFC.rin - vFFC.lastRin) + Kb * (vFFC.rin - 2 * vFFC.lastRin + vFFC.perrRin);
// 	vFFC.output = result;
// 	vFFC.perrRin = vFFC.lastRin;
// 	vFFC.lastRin = vFFC.rin;
// }

